/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_low
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// Schunk_low
#include <schunk_low/schunk.h>

// General
#include<string>
#include<vector>

// ROS
#include <ros/ros.h>

// Library for communicating with the hardware
#include <schunk_libm5api/m5apiw32.h>

// Namespace std
using namespace std;

/******************************************************************************
             METHODS FOR STORING ROBOT'S INFORMATION IN MESSAGES
 *****************************************************************************/

schunk_msgs::AllParameters Schunk::ReadAllParameters()
{
    float float_data;
    long long_data;
    short short_data;
    int int_data;
    unsigned long ulong_data;
    unsigned short ushort_data;
    unsigned char uchar_data;
    const char* pcchar_data;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getMinPos(device_, module_ids_[i], &float_data);
        param_msg_.MinPos[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMinPosInc(device_, module_ids_[i], &long_data);
        param_msg_.MaxPos[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxPos(device_, module_ids_[i], &float_data);
        param_msg_.MaxPos[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxPosInc(device_, module_ids_[i], &long_data);
        param_msg_.MaxPosInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxVel(device_, module_ids_[i], &float_data);
        param_msg_.MaxVel[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxVelInc(device_, module_ids_[i], &long_data);
        param_msg_.MaxVelInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxAcc(device_, module_ids_[i], &float_data);
        param_msg_.MaxAcc[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxAccInc(device_, module_ids_[i], &long_data);
        param_msg_.MaxAccInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getHomeOffset(device_, module_ids_[i], &float_data);
        param_msg_.HomeOffset[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getHomeOffsetInc(device_, module_ids_[i], &long_data);
        param_msg_.HomeOffsetInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getHomeVel(device_, module_ids_[i], &float_data);
        param_msg_.HomeVel[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getHomeVelInc(device_, module_ids_[i], &long_data);
        param_msg_.HomeVelInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxCur(device_, module_ids_[i], &float_data);
        param_msg_.MaxCur[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxDeltaPos(device_, module_ids_[i], &float_data);
        param_msg_.MaxDeltaPos[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxDeltaPosInc(device_, module_ids_[i], &long_data);
        param_msg_.MaxDeltaPosInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getA0(device_, module_ids_[i], &short_data);
        param_msg_.A0[i] = short_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getC0(device_, module_ids_[i], &short_data);
        param_msg_.C0[i] = short_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDamp(device_, module_ids_[i], &short_data);
        param_msg_.Damp[i] = short_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMinLogicVoltage(device_, module_ids_[i], &float_data);
        param_msg_.MinLogicVoltage[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxLogicVoltage(device_, module_ids_[i], &float_data);
        param_msg_.MaxLogicVoltage[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMinMotorVoltage(device_, module_ids_[i], &float_data);
        param_msg_.MinMotorVoltage[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxMotorVoltage(device_, module_ids_[i], &float_data);
        param_msg_.MaxMotorVoltage[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getNominalMotorCurrent(device_, module_ids_[i], &float_data);
        param_msg_.NominalMotorCurrent[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaximumMotorCurrent(device_, module_ids_[i], &float_data);
        param_msg_.MaximumMotorCurrent[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getLogicUndershootTime(device_, module_ids_[i], &long_data);
        param_msg_.LogicUndershootTime[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getLogicOvershootTime(device_, module_ids_[i], &long_data);
        param_msg_.LogicOvershootTime[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMotorUndershootTime(device_, module_ids_[i], &long_data);
        param_msg_.MotorUndershootTime[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMotorOvershootTime(device_, module_ids_[i], &long_data);
        param_msg_.MotorOvershootTime[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getNomCurOvershootTime(device_, module_ids_[i], &long_data);
        param_msg_.NomCurOvershootTime[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getHMaxCurOvershootTime(device_, module_ids_[i], &long_data);
        param_msg_.HMaxCurOvershootTime[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getCurrentLimit(device_, module_ids_[i], &float_data);
        param_msg_.CurrentLimit[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getKpPWMLimit(device_, module_ids_[i], &long_data);
        param_msg_.KpPWMLimit[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxPWMOutput(device_, module_ids_[i], &long_data);
        param_msg_.MaxPWMOutput[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getModuleType(device_, module_ids_[i], &uchar_data);
        param_msg_.ModuleType[i] = uchar_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getModuleVersion(device_, module_ids_[i], &ushort_data);
        param_msg_.ModuleVersion[i] = ushort_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getModuleSerialNo(device_, module_ids_[i], &ulong_data);
        param_msg_.ModuleSerialNo[i] = (unsigned int)ulong_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getLoadLimit(device_, module_ids_[i], &long_data);
        param_msg_.LoadLimit[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getMaxLoadGradient(device_, module_ids_[i], &long_data);
        param_msg_.MaxLoadGradient[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getLoadDeltaTime(device_, module_ids_[i], &ushort_data);
        param_msg_.LoadDeltaTime[i] = ushort_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefCANBaudRate(device_, module_ids_[i], &uchar_data);
        param_msg_.DefCANBaudRate[i] = uchar_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefRSBaudRate(device_, module_ids_[i], &uchar_data);
        param_msg_.DefRSBaudRate[i] = uchar_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefBaudRate(device_, module_ids_[i], &uchar_data);
        param_msg_.DefBaudRate[i] = uchar_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefBurnCount(device_, module_ids_[i], &uchar_data);
        param_msg_.DefBurnCount[i] = uchar_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefGearRatio(device_, module_ids_[i], &float_data);
        param_msg_.DefGearRatio[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefLinearRatio(device_, module_ids_[i], &float_data);
        param_msg_.DefLinearRatio[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefCurRatio(device_, module_ids_[i], &float_data);
        param_msg_.DefCurRatio[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefIncPerTurn(device_, module_ids_[i], &ulong_data);
        param_msg_.DefIncPerTurn[i] = (unsigned int)ulong_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefBrakeTimeOut(device_, module_ids_[i], &ushort_data);
        param_msg_.DefBrakeTimeOut[i] = ushort_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefMinPos(device_, module_ids_[i], &float_data);
        param_msg_.DefMinPos[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefMaxPos(device_, module_ids_[i], &float_data);
        param_msg_.DefMaxPos[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefMaxVel(device_, module_ids_[i], &float_data);
        param_msg_.DefMaxVel[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefMaxAcc(device_, module_ids_[i], &float_data);
        param_msg_.DefMaxAcc[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefHomeOffset(device_, module_ids_[i], &float_data);
        param_msg_.DefHomeOffset[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefHomeVel(device_, module_ids_[i], &float_data);
        param_msg_.DefHomeVel[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefMaxCur(device_, module_ids_[i], &float_data);
        param_msg_.DefMaxCur[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefMaxDeltaPos(device_, module_ids_[i], &float_data);
        param_msg_.DefMaxDeltaPos[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefA0(device_, module_ids_[i], &short_data);
        param_msg_.DefA0[i] = short_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefC0(device_, module_ids_[i], &short_data);
        param_msg_.DefC0[i] = short_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDefDamp(device_, module_ids_[i], &short_data);
        param_msg_.DefDamp[i] = short_data;
        pthread_mutex_unlock(&mutex_);
    }

    pthread_mutex_lock(&mutex_);
    int_data = PCube_getDeviceCount();
    param_msg_.DeviceCount = int_data;
    pthread_mutex_unlock(&mutex_);

    pthread_mutex_lock(&mutex_);
    PCube_getDeviceIdMap(&int_data);
    param_msg_.DeviceIdMap = int_data;
    pthread_mutex_unlock(&mutex_);

    pthread_mutex_lock(&mutex_);
    pcchar_data = PCube_getDeviceRevision(device_);
    param_msg_.DeviceRevision = pcchar_data;
    pthread_mutex_unlock(&mutex_);

    pthread_mutex_lock(&mutex_);
    pcchar_data = PCube_getDeviceInitString(device_);
    param_msg_.DeviceInitString = pcchar_data;
    pthread_mutex_unlock(&mutex_);

    pthread_mutex_lock(&mutex_);
    pcchar_data = PCube_getDeviceName(device_);
    param_msg_.DeviceName = pcchar_data;
    pthread_mutex_unlock(&mutex_);

    pthread_mutex_lock(&mutex_);
    int_data = PCube_getModuleCount(device_);
    param_msg_.ModuleCount = int_data;
    pthread_mutex_unlock(&mutex_);

    pthread_mutex_lock(&mutex_);
    int_data = PCube_getDllVersion();
    param_msg_.DllVersion = int_data;
    pthread_mutex_unlock(&mutex_);

    return param_msg_;
}
